/* gs_core.c -- Arquivo core do gearserial
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <time.h>
#include <string.h>

#include "gs_core.h"
#include "xerror.h"
#include "xmalloc.h"

/* Static */
static void gs_core_update_checksun( gs_core_t * self );
static void gs_core_send_command( gs_core_t * self );
static void gs_core_set_conf( gs_core_t * self, const char conf );

/** Cria um novo objeto Core que é renponsável por passar os comandos
 * para a serial.
 *
 * @param *serial_path Caminho para a serial do computador.
 *
 * @return Retorna um novo objeto Core.
 */
gs_core_t *
gs_core_new( const char * serial_path )
{
    gs_core_t * self = NULL;

    self = XMALLOC( gs_core_t, 1 );

    self->path = XMALLOC( char, strlen(serial_path) + 1);
    strcpy( self->path, serial_path );

    self->fd = open(self->path, O_RDWR | O_NOCTTY | O_NDELAY);
    if (self->fd == -1)
    {
	xerror_error("Naum consegue abrir a serial");
	return NULL;
    }

    self->speed = B9600;
    
    tcgetattr (self->fd, &(self->options));

    /* velocidade */
    cfsetspeed (&(self->options), self->speed);

    /* 8 bits por byte */
    self->options.c_cflag |= CS8;

    tcsetattr (self->fd, TCSANOW, &(self->options) );

    self->command_size = 9;
    self->command = XMALLOC( unsigned char, self->command_size );

    /* DEFINIÇÃO DA PALAVRA DO PROTOCOLO */
    /* BYTE 1 - primeiro byte do endereço */
    self->command[0] = 'B';

    /* BYTE 2 - segundo byte do endereço */
    self->command[1] = 'A';

    /* BYTE 3 - terceiro byte do endereço */
    self->command[2] = 'L';

    /* BYTE 4 - byte de configuração, que engloba o chute, o rolinho e os sentidos dos motores */
    self->conf = &self->command[3];
    self->command[3] = 0x01;

    /* BYTE 5 - velocidade do motor 1 */
    self->motor_1 = &self->command[4];
    self->command[4] = 0x00;

    /* BYTE 6 - velocidade do motor 2 */
    self->motor_2 = &self->command[5];
    self->command[5] = 0x00;

    /* BYTE 7 - velocidade do motor 3 */
    self->motor_3 = &self->command[6];
    self->command[6] = 0x00;

    /* BYTE 8 - checksum high */
    self->command[7] = 0x00;

    /* BYTE 9 - checksum low */
    self->command[8] = 0x00;
    /* FIM DA DEFINIÇÃO DA PALAVRA DO PROTOCOLO */

    self->mascara1 = 0x000000FF;
    self->mascara2 = 0x0000FF00;

    self->delay.tv_sec = 0;
    self->delay.tv_nsec = 10000;

    self->command_repeat = 5;
    
    return self;
}

/** Liga ou desliga o sistema de drible.
 *
 * @param *self Objeto core.
 *
 * @param enable Variável TRUE ou FALSE que liga e desliga o sistema
 * de drible.
 */
void
gs_core_robot_dribble_enable( gs_core_t * self, int enable )
{
    int repeat = 0;

    if( enable )
      *(self->conf) ^= 0x80;
    else
      *(self->conf) &= 0x7F;

    gs_core_update_checksun( self );

    repeat = self->command_repeat;
    self->command_repeat = 1;
    gs_core_send_command( self );
    self->command_repeat = repeat;
}

/** Chuta.
 *
 * @param *self Objeto Core.
 */
void
gs_core_robot_kick( gs_core_t * self )
{
//    unsigned char saved_conf = 0;
    int repeat = 0;
   
    *(self->conf) ^= 0x40;

    gs_core_update_checksun( self );

    repeat = self->command_repeat;
    self->command_repeat = 1;
    gs_core_send_command( self );
    self->command_repeat = repeat;
    
    *(self->conf) &= 0xBF;
}

/** Move o robo para frente acionando as duas rodas laterias.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gs_core_set_robot_forward( gs_core_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gs_core_set_conf( self, 0x02);

    gs_core_update_checksun( self );
    gs_core_send_command( self );
}

/** Move o robo para tras acionando as duas rodas laterias.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gs_core_set_robot_backward( gs_core_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gs_core_set_conf( self, 0x04);

    gs_core_update_checksun( self );
    gs_core_send_command( self );
}

/** Gira o robô para a esquerda.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gs_core_set_robot_turn_left( gs_core_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gs_core_set_conf( self, 0x00);

    gs_core_update_checksun( self );
    gs_core_send_command( self );
}

/** Gira o robô para a direita.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gs_core_set_robot_turn_right( gs_core_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gs_core_set_conf( self, 0x07);

    gs_core_update_checksun( self );
    gs_core_send_command( self );
}

/** Para o robô.
 *
 * @param *self Objeto Core.
 */
void
gs_core_set_robot_stop( gs_core_t * self )
{
    *(self->motor_1) = 0x00;
    *(self->motor_2) = 0x00;
    *(self->motor_3) = 0x00;
    gs_core_set_conf( self, 0x00 );

    gs_core_update_checksun( self );
    gs_core_send_command( self );
}

static void
gs_core_set_conf( gs_core_t * self, const char conf )
{
  *(self->conf) &= 0xF0;
  *(self->conf) ^= conf;
}

/** Envia o comando pela serial.
 *
 * @param *core Objeto.
 */
static void
gs_core_send_command( gs_core_t * self )
{
    int i = 0;

    for (i = 0; i < self->command_repeat; i++)
    {
	if( write (self->fd, self->command, self->command_size) < 0)
	    xerror_error("Nao consegui escrever na serial");	
	nanosleep (&(self->delay), &(self->sobrando) );
    }

}

/** Atualiza o checksun na estrutura
 *
 * @param *core Estrutura Core.
 */
static void
gs_core_update_checksun( gs_core_t * self )
{
    unsigned int total = 0;
    unsigned int roda = 0;
    int k = 0;
    
    /* Checksum */
    for (k = 0; k < self->command_size - 2; k++)
    {
	total += self->command[k];
    }
    roda = total & self->mascara2;
    roda = roda >> 8;
    self->command[7] = roda;
    self->command[8] = total & self->mascara1;
}

/** Destroi um objeto Core.
 *
 * @param *self O objeto.
 */
void
gs_core_destroy( gs_core_t * self )
{
    XFREE( self->path );
    XFREE( self->command );

    close( self->fd );
    
    XFREE( self );
}
